/**
 * @file AX_12.c
 * @author  Nguyen Ngoc The Vinh < vsop_valentino@yahoo.com >
 * @version 1.0
 * @date 26-09-2012
 *
 * @copyright
 * This poject and all of its relevant documents, source codes, compiled libraries are belong
 * to <b> Smart Sensing and Intelligent Controlling Group (SSAIC)</b> \n
 * You are prohibited to broadcast, distribute, copy, modify, print, or reproduce this in anyway
 * without written permission from SSAIC \n
 * <b> Copyright (C) 2012 by SSAIC, All Right Reversed </b>
 *
 * @brief
 * Implement hardware abstract layer to control AX-12 servo
 * @attention
 */
#include "AX_12.h"
#include "hl_uart_c.h"

//=========================================================//
//====================Private variables====================//
//=========================================================//
static uint8_t	sendBuffer[16];
static uint8_t  recvBuffer[16];
extern uint8_t	*status;


//=========================================================//
//====================Public functions=====================//
//=========================================================//

//=========================================================//
//=================SET values functions====================//
//=========================================================//
/** 
 * @brief Set ID
 * Implement ID determine Dynamixel actuators
 * ID have 254 value
 * Range from 0x00 to 0xFE
 * @attention
 */
uint8_t		AX_12::SetID(uint8_t servoId, uint8_t ID)  
{
	AX12.Write(servoId, AX12_ID_REG, ID);								
    return AX12_OK;
}

uint8_t		AX_12::SetBaudRate(uint8_t servoId, uint8_t BaudRate)
{
    /*if (BaudRate > 254)					
        return AX12_FAIL;
    AX12.Write(servoId, AX12_BAUD_RATE_REG, BaudRate);*/
    return AX12_OK;	
}

/**
 * @brief Set Return Delay Time
 * Implement delay time takes for the Status Packet to return after the Instruction Packet is sent
 * Time is given by (2uSec * Address 5 value) 
 * Initial value is 0xFA (250)
 * @attention
 */
uint8_t		AX_12::SetReturnDelayTime(uint8_t servoId, uint8_t delayTime)
{
	AX12.Write(servoId, AX12_RETURN_DELAY_TIME_REG, delayTime);
	return AX12_OK;
}

/** 
 * @brief Set CW Angle Limit
 * Sets the Dynamixel actuator's operating angle range.
 * The Goal position needs to be within the range of: CW Angle Limt <= Goal Position <= CCW Angle Limt
 * An angle Limit error will occur if the Goal Position is set out side this range set by the operating angle limits
 * AngleLimit range is between 0 - 300*
 * @attention
 */

uint16_t	AX_12::SetCWAngleLimit(uint8_t servoId, uint16_t angleLimit)
{
	AX12.Write_16bits(servoId, AX12_CW_ANGLE_LIMIT_L_REG, angleLimit);
	return AX12_OK;
}

uint16_t	AX_12::SetCCWAngleLimit(uint8_t servoId, uint16_t angleLimit)
{
	AX12.Write_16bits(servoId, AX12_CCW_ANGLE_LIMIT_L_REG, angleLimit);
	return AX12_OK;
}

/** 
 * @brief Set Temperature Limit
 * The upper limit of the DA operating temperature
 * If the internal temperature of the DA gets higher than this value
 * The over Heating Wrror Bit (bit 2 of Status Packet) will return the value 1
 * And an alarm will be set by Address 17, 18. The values are in Degrees Celsius.
 * @attention
 */
uint8_t		AX_12::SetTemperatureLimit(uint8_t servoId, uint8_t temperatureLimit)
{
	AX12.Write(servoId, AX12_Present_Temperature_REG, temperatureLimit);
	return AX12_OK;
}

uint8_t		AX_12::SetLowestVoltageLimit(uint8_t servoId, uint8_t voltageLimit)
{
	if(voltageLimit < 50) 
		return AX12_FAIL;
	AX12.Write(servoId, AX12_Lowest_Limit_Voltage_REG, voltageLimit);
	return AX12_OK;
}

uint8_t		AX_12::SetHighestVoltageLimit(uint8_t servoId, uint8_t voltageLimit)
{
	if(voltageLimit > 250)
		return AX12_FAIL;
	AX12.Write(servoId, AX12_Highest_Limit_Voltage_REG, voltageLimit);
	return AX12_OK;
}

uint16_t	AX_12::SetMaxTorque(uint8_t servoId, uint16_t maxTorque)
{
	AX12.Write_16bits(servoId, AX12_Max_Torque_L_REG, maxTorque);
	return AX12_OK;
}

/**
 * @brief Set Status Return Level
 * Determines whether the DA will return a Status Packet after receiving an Instruction Packet
 * Level 0 : the Broadcast ID (0XFE) the Status Packet won't be returned regardless of the Address 0x10 value
 * Level 1 : respond only to READ DATA instructions, parameter numbers of READ DATA = 2
 * Level 2 : respond to all instructions
 * @attention
 */
uint8_t		AX_12::SetStatusReturnLevel(uint8_t servoId, uint8_t returnLevel)
{
	if(servoId == 0xFE) return AX12_OK;										
	if(returnLevel == 0) {
		AX12.Write(servoId, AX12_Status_Return_Level_REG, 0);
	}
	else if(returnLevel == 1) {													
		AX12.Write(servoId, AX12_Status_Return_Level_REG, 1);					
	}
	else if(returnLevel == 2) {													
		AX12.Write(servoId, AX12_Status_Return_Level_REG, 2);	
	}
	else
		return AX12_FAIL;
	return AX12_OK;
}

/**
 * @brief Set Alarm Led
 * If the corresponding Bit is set to 1, the LED blinks when an Error occurs
 * Bit 0 : if set to 1, the LED blinks when an Input Voltage Error occurs
 * Bit 1 : if set to 1, the LED blinks when an Angle Limit Error occurs
 * Bit 2 : if set to 1, the LED blinks when an Overheating Error occurs
 * Bit 3 : if set to 1, the LED blinks when an Range Error occurs
 * Bit 4 : if set to 1, the LED blinks when an Checksum Error occurs
 * Bit 5 : if set to 1, the LED blinks when an Overload Error occurs
 * Bit 6 : if set to 1, the LED blinks when an Instruction Error occurs
 * Bit 7 : 0
 * @attention
 */
uint8_t		AX_12::SetAlarmLed(uint8_t servoId, uint8_t alarmLed)
{
	if(alarmLed > 0x7F)
		return AX12_FAIL;
	else
		AX12.Write(servoId, AX12_Alarm_Led_REG, alarmLed);
	return AX12_OK;
}

uint8_t		AX_12::SetAlarmShutdown(uint8_t servoId, uint8_t alarmShutdown)
{
	if(alarmShutdown > 0x7F)
		return AX12_FAIL;
	else 
		AX12.Write(servoId, AX12_Alarm_Shutdown_REG, alarmShutdown);
	return AX12_OK;
}

/**
 * @brief Set Torque Enable
 * When the power is first turned on, the DA enters the Torque Free Run condition (zero torque)
 * Setting the value in Address 0x18 to 1 enables the torque
 * @attention
 */
uint8_t		AX_12::SetTorqueEnable(uint8_t servoId, uint8_t torqueEnable) 
{
	if(torqueEnable == 1) {
		AX12.Write(servoId, AX12_Torque_Enable_REG, 1);
		return AX12_OK;
	}
	return AX12_FAIL;
}

/**
 * @brief Set LED
 * The led turns on when set to 1 and turns off when set to 0
 * @attention
 */
uint8_t		AX_12::SetLed(uint8_t servoId, uint8_t ledVal)
{
	if(ledVal == 0)
		AX12.Write(servoId, AX12_LED_REG, 0);
	else if(ledVal == 1)
		AX12.Write(servoId, AX12_LED_REG, 1);
	else
		return AX12_FAIL;
	return AX12_OK;
}
/**
 * @brief Set Compliance Margin and Slope
 * Define by setting the Margin and Slope. This feature can be utilized for absorbing shocks at the output shaft
 * @attention
 */
uint8_t		AX_12::SetCWComplianceMargin(uint8_t servoId, uint8_t complianceMargin)
{
	AX12.Write(servoId, AX12_CW_Compliance_Margin_REG, complianceMargin);
	return AX12_OK;
}

uint8_t		AX_12::SetCCWComplianceMargin(uint8_t servoId, uint8_t complianceMargin)
{
	AX12.Write(servoId, AX12_CCW_Compliance_Margin_REG, complianceMargin);
	return AX12_OK;
}

uint8_t		AX_12::SetCWComplianceSlope(uint8_t servoId, uint8_t complianceSlope)
{
	AX12.Write(servoId, AX12_CW_Compliance_Slope_REG, complianceSlope);
	return AX12_OK;
}

uint8_t		AX_12::SetCCWComplianceSlope(uint8_t servoId, uint8_t complianceSlope)
{
	AX12.Write(servoId, AX12_CCW_Compliance_Slope_REG, complianceSlope);
	return AX12_OK;
}
/**
 * @brief Set Goal Position
 * Request angular position for the Dynamixel actuator output to move to
 * Range from 0x00 to 0x3ff
 * @attention
 */
uint16_t	AX_12::SetGoalPosition(uint8_t servoId, uint16_t goalPosition)
{
	if (goalPosition >= 0 && goalPosition <= 0x3FF){
		AX12.Write_16bits(servoId, AX12_Goal_Position_L_REG, goalPosition);}
	else
		return AX12_FAIL;
	return AX12_OK;
}
/**
 * @brief Set Moving Speed
 * Sets the angular velocity of the output moving to the Goal Position
 * Range from 1 to 0x3ff (114 RPM)
 * @attention
 */
uint16_t	AX_12::SetMovingSpeed(uint8_t servoId, uint16_t movingSpeed)
{
	if (movingSpeed >=1 && movingSpeed <= 0x3ff)
		AX12.Write_16bits(servoId, AX12_Moving_Speed_L_REG, movingSpeed);
	else
		return AX12_FAIL;
	return AX12_OK;
}
/**
 * @brief Set Torque Limit
 * Request angular torque for the Dynamixel actuator output to move to 
 * Range from 0 to 0x3ff
 */
uint16_t	AX_12::SetTorqueLimit(uint8_t servoId, uint16_t torqueLimit)
{
	if (torqueLimit >= 0 && torqueLimit <= 0x3ff)
		AX12.Write_16bits(servoId, AX12_Torque_Limit_L_REG, torqueLimit);
	else
		return AX12_FAIL;
	return AX12_OK;
}
/**
 * @brief Registered Instruction
 * Set 1 when  a instruction is assigned REG_WRITE command
 * Set 0 when after it completes the assigned instruction by the Action command
 */
uint8_t		AX_12::SetRegisteredInstruction(uint8_t servoId, uint8_t instruction)
{
	AX12.Write(servoId, AX12_Registered_Instruction_REG, instruction);
	return AX12_OK;
}
/**
 * @brief Set Lock 
 * Set 1, only address 0x18 to 0x23 can be written to 
 * Once locked, it can be unlocked by turning the power off 
 */
uint8_t		AX_12::SetLock(uint8_t servoId, uint8_t lockMode)
{
	AX12.Write(servoId, AX12_Lock_REG, lockMode);
	return AX12_OK;
}
/**
 * @brief Set Punch
 * The minimum current supplied to the motor during operation
 * The value is range from 0x20 to 0x3ff
 */
uint16_t	AX_12::SetPunch(uint8_t servoId, uint16_t punch)
{
	if (punch >= 0x20 && punch <= 0x3ff)
		AX12.Write_16bits(servoId, AX12_Punch_L_REG, punch);
	else
		return AX12_FAIL;
	return AX12_OK;
}


//=========================================================//
//=================GET values functions====================//
//=========================================================//

uint16_t	AX_12::GetModelNum(uint8_t servoId)
{
	AX12.Read(servoId, AX12_MODEL_NUM_H_REG, 2);
	return AX12_OK;
}

uint8_t		AX_12::GetFirmwareVersion(uint8_t servoId)
{
	AX12.Read(servoId, AX12_FIRM_VER_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetId(uint8_t servoId)
{
	AX12.Read(servoId, AX12_ID_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetBaudRate(uint8_t servoId)
{
	AX12.Read(servoId, AX12_BAUD_RATE_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetReturnDelayTime(uint8_t servoId)
{
	AX12.Read(servoId, AX12_RETURN_DELAY_TIME_REG, 1);
	return AX12_OK;
}

uint16_t	AX_12::GetCWAngleLimit(uint8_t servoId)
{
	AX12.Read(servoId, AX12_CW_ANGLE_LIMIT_L_REG, 2);
	return AX12_OK;
}

uint16_t	AX_12::GetCCWAngleLimit(uint8_t servoId)
{
	AX12.Read(servoId, AX12_CCW_ANGLE_LIMIT_L_REG, 2);
	return AX12_OK;	
}

uint8_t		AX_12::GetTemperatureLimit(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Highest_Limit_Temperature_REG, 1);
	return AX12_OK;	
}

uint8_t		AX_12::GetLowestVoltageLimit(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Lowest_Limit_Voltage_REG, 1);
	return AX12_OK;		
}

uint8_t		AX_12::GetHighestVoltageLimit(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Highest_Limit_Voltage_REG, 1);
	return AX12_OK;		
}

uint16_t	AX_12::GetMaxTorque(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Max_Torque_L_REG, 2);
	return AX12_OK;	
}

uint8_t		AX_12::GetStatusReturnLevel(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Status_Return_Level_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetAlarmLED(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Alarm_Led_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetAlarmShutdown(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Alarm_Shutdown_REG, 1);
	return AX12_OK;	
}

uint16_t	AX_12::GetDownCalibration(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Down_Calibration_L_REG, 2);
	return AX12_OK;	
}

uint16_t	AX_12::GetUpCalibration(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Up_Calibration_L_REG, 2);
	return AX12_OK;	
}

uint8_t		AX_12::GetTorqueEnable(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Torque_Enable_REG, 1);
	return AX12_OK;	
}

uint8_t		AX_12::GetLed(uint8_t servoId)
{
	AX12.Read(servoId, AX12_LED_REG, 1);
	return AX12_OK;	
}

uint8_t		AX_12::GetCWComplianceMargin(uint8_t servoId)
{
	AX12.Read(servoId, AX12_CW_Compliance_Margin_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetCCWComplianceMargin(uint8_t servoId)
{
	AX12.Read(servoId, AX12_CCW_Compliance_Margin_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetCWComplianceSlope(uint8_t servoId)
{
	AX12.Read(servoId, AX12_CW_Compliance_Slope_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetCCWComplianceSlope(uint8_t servoId)
{
	AX12.Read(servoId, AX12_CCW_Compliance_Slope_REG, 1);
	return AX12_OK;
}

uint16_t	AX_12::GetGoalPosition(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Goal_Position_L_REG, 2);
	return AX12_OK;
}

uint16_t	AX_12::GetMovingSpeed(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Moving_Speed_L_REG, 2);
	return AX12_OK;
}

uint16_t	AX_12::GetTorqueLimit(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Torque_Limit_L_REG, 2);
	return AX12_OK;
}

uint16_t	AX_12::GetPresentPosition(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Present_Position_L_REG, 2);
	return AX12_OK;
}

uint16_t	AX_12::GetPresentSpeed(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Present_Speed_L_REG,2);
	return AX12_OK;
}

uint16_t	AX_12::GetPresentLoad(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Present_Load_L_REG, 2);
	return AX12_OK;
}

uint8_t		AX_12::GetPresentVoltage(uint8_t servoId)
{
	AX12.Read(servoId , AX12_Present_Voltage_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetPresentTemperature(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Present_Temperature_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetRegisteredInstruction(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Registered_Instruction_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetMoving(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Moving_REG, 1);
	return AX12_OK;
}

uint8_t		AX_12::GetLock(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Lock_REG, 1);
	return AX12_OK;
}

uint16_t	AX_12::GetPunch(uint8_t servoId)
{
	AX12.Read(servoId, AX12_Punch_L_REG, 2);
	return AX12_OK;
}

//=========================================================//
//==================SEND values functions==================//
//=========================================================//

/**
  * @brief Send PING instruction to a AX-12 servo and wait its response
  * @param servoId ID of the servo receiving the instruction
  * @param status Status information extracted from the response packet 
  * @retval AX12_COLLISION Collision is detected while sendding the instruction
  * @retval AX12_NO_RESPONSE Doesn't receive response from the servo receiving the instruction
  * @retval AX12_CHECKSUM_ERROR Receive a packet with checksum error
  * @retval AX12_WRONG_ID Receive a packet with the ID differ with the received servo
  * @retval AX12_OK Send the instruction and receive response successful
  */
uint8_t		AX_12::SendPing(uint8_t servoId, uint8_t* status){
    sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;            			// ID
    sendBuffer[3] = 2;                				// Length
    sendBuffer[4] = AX12_PING_INSTR;     			// Instruction Ping
    sendBuffer[5] = ~((uint8_t) (servoId + 2 + AX12_PING_INSTR));  // Check Sum
	BOARD_USARTSendBuff.Out(sendBuffer, 6);
	// check collision
	// wait response
	// check checksum
	// check recvId
	// assign value for status
	return AX12_OK;
}

/**
  * @brief Send READ one byte instruction to a AX-12 servo
  * @retval AX12_OK Send read data one byte successful
  */
uint8_t     AX_12::SendReadDataOneByte(uint8_t servoId, uint8_t address)
{
	AX12.Read(servoId, address, 1);
	return AX12_OK;
}

/**
  * @brief Send READ one byte instruction to a AX-12 servo
  * @retval AX12_OK Send read data two bytes successful
  */
uint16_t    AX_12::SendReadDataTwoByte(uint8_t servoId, uint8_t address)
{
	AX12.Read(servoId, address, 2);
	return AX12_OK;
}

/**
  * @brief Send READ instruction to a AX-12 servo
  * @param servoId ID of the servo receiving the instruction
  * @param address information extracted from the response packet 
  * @param Len of the data to be read
  */
void	AX_12::Read(uint8_t servoId, uint8_t address, uint8_t Len)
{
    sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;
    sendBuffer[3] = 4;					//Length
    sendBuffer[4] = 0x02;				//Instruction Read DATA
    sendBuffer[5] = address;            //Register Address
    sendBuffer[6] = Len;                //Length of the data to be read
    sendBuffer[7] = ~((uint8_t)(servoId + 4 + 2 + address + Len));
	BOARD_USARTSendBuff.Out(sendBuffer, 8);
    //BOARD_USARTSendBuff(sendBuffer, 8);
    return;
}

/**
  * @brief Send WRITE DATA instruction to a AX-12 servo and wait its response
  * @param servoId ID of the servo receiving the instruction
  * @param status Status information extracted from the response packet 
  * @retval AX12_COLLISION Collision is detected while sendding the instruction
  * @retval AX12_NO_RESPONSE Doesn't receive response from the servo receiving the instruction
  * @retval AX12_CHECKSUM_ERROR Receive a packet with checksum error
  * @retval AX12_WRONG_ID Receive a packet with the ID differ with the received servo
  * @retval AX12_OK Send the instruction and receive response successful
  */
uint8_t  	AX_12::SendWriteDataOneByte(uint8_t servoId, uint8_t address, uint8_t value){
	sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;                	// ID
    sendBuffer[3] = 4;                			// Length
    sendBuffer[4] = AX12_WRITE_DATA_INSTR; 		// Instruction Write Data
    sendBuffer[5] = address;    				// Register Address
    sendBuffer[6] = value;        				// Register Value
    sendBuffer[7] = ~((uint8_t) (servoId + 4 + AX12_WRITE_DATA_INSTR + address + value));        // Check Sum
    BOARD_USARTSendBuff.Out(sendBuffer, 8);
	// check collision
	// wait response
	// check checksum
	// check recvId
	// assign value for status
    return AX12_OK;
}


/**
  * @brief Send WRITE DATA instruction to a AX-12 servo and wait its response
  * @param servoId ID of the servo receiving the instruction
  * @param status Status information extracted from the response packet 
  * @retval AX12_COLLISION Collision is detected while sendding the instruction
  * @retval AX12_NO_RESPONSE Doesn't receive response from the servo receiving the instruction
  * @retval AX12_CHECKSUM_ERROR Receive a packet with checksum error
  * @retval AX12_WRONG_ID Receive a packet with the ID differ with the received servo
  * @retval AX12_OK Send the instruction and receive response successful
  */
uint8_t  	AX_12::SendWriteDataTwoBytes(uint8_t servoId, uint8_t address, uint16_t value){

    sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;                		// ID
    sendBuffer[3] = 5;                				// Length
    sendBuffer[4] = AX12_WRITE_DATA_INSTR;     		// Instruction Write Data
    sendBuffer[5] = address;    					// Register Address
    sendBuffer[6] = (uint8_t) (value & 0xFF);   	// Low register value
    sendBuffer[7] = (uint8_t) (value >> 8) ;     	// High register value
    sendBuffer[8] = ~((uint8_t)(servoId + 5 + AX12_WRITE_DATA_INSTR + address + sendBuffer[6] + sendBuffer[7]));        // Check Sum
    BOARD_USARTSendBuff.Out(sendBuffer, 9);
	//BOARD_USARTSendBuff(sendBuffer, 9);
	// check collision
	// wait response
	// check checksum
	// check recvId
	// assign value for status
    return AX12_OK;
}

/**
  * @brief Send WRITE DATA MultiBytes instruction to a AX-12 servo and wait its response
  * @param servoId ID of the servo receiving the instruction
  * @param status Status information extracted from the response packet 
  * @retval AX12_COLLISION Collision is detected while sendding the instruction
  * @retval AX12_NO_RESPONSE Doesn't receive response from the servo receiving the instruction
  * @retval AX12_CHECKSUM_ERROR Receive a packet with checksum error
  * @retval AX12_WRONG_ID Receive a packet with the ID differ with the received servo
  * @retval AX12_OK Send the instruction and receive response successful
  */
uint8_t  	AX_12::SendWriteDataMultiBytes(uint8_t servoId, uint8_t address, uint8_t value[], uint8_t length)
{
    sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;                		// ID
    sendBuffer[3] = 3 + length;                		// Length
    sendBuffer[4] = AX12_WRITE_DATA_INSTR;     		// Instruction Write Data
    sendBuffer[5] = address;    					// Register Address	
	int i = 0;
	int sum = (uint8_t)(servoId + 3 + length + AX12_WRITE_DATA_INSTR + address);
	while(value[i] != NULL){
		sendBuffer[i+6] = (uint8_t) (value[i] & 0xFF);		
		sum = (uint8_t)(sum + sendBuffer[i+6]);		// Check Sum
		i++;
	}
	sendBuffer[i+6] = ~((uint8_t) sum);
    BOARD_USARTSendBuff.Out(sendBuffer, i+7);
	// check collision
	// wait response
	// check checksum
	// check recvId
	// assign value for status	 */
    return AX12_OK;		  
}

/**
  * @brief Send REG WRITE DATA one byte instruction to a AX-12 servo and wait its response
  * @param servoId ID of the servo receiving the instruction
  * @param status Status information extracted from the response packet 
  * @retval AX12_COLLISION Collision is detected while sendding the instruction
  * @retval AX12_NO_RESPONSE Doesn't receive response from the servo receiving the instruction
  * @retval AX12_CHECKSUM_ERROR Receive a packet with checksum error
  * @retval AX12_WRONG_ID Receive a packet with the ID differ with the received servo
  * @retval AX12_OK Send the instruction and receive response successful
  */

uint8_t  	AX_12::SendWriteRegOneByte(uint8_t servoId, uint8_t address, uint8_t value)
{
	sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;                	// ID
    sendBuffer[3] = 4;                			// Length
    sendBuffer[4] = AX12_WRITE_REG_INSTR; 		// Instruction Reg Write Data
    sendBuffer[5] = address;    				// Register Address
    sendBuffer[6] = value;        				// Register Value
    sendBuffer[7] = ~((uint8_t) (servoId + 4 + AX12_WRITE_DATA_INSTR + address + value));        // Check Sum
	BOARD_USARTSendBuff.Out(sendBuffer, 8);
	// check collision
	// wait response
	// check checksum
	// check recvId
	// assign value for status
    return AX12_OK;	
}
 
/**
  * @brief Send REG WRITE DATA two byte instruction to a AX-12 servo and wait its response
  * @param servoId ID of the servo receiving the instruction
  * @param status Status information extracted from the response packet 
  * @retval AX12_COLLISION Collision is detected while sendding the instruction
  * @retval AX12_NO_RESPONSE Doesn't receive response from the servo receiving the instruction
  * @retval AX12_CHECKSUM_ERROR Receive a packet with checksum error
  * @retval AX12_WRONG_ID Receive a packet with the ID differ with the received servo
  * @retval AX12_OK Send the instruction and receive response successful
  */

uint8_t  	AX_12::SendWriteRegTwoBytes(uint8_t servoId, uint8_t address, uint16_t value)
{
    sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;                		// ID
    sendBuffer[3] = 5;                				// Length
    sendBuffer[4] = AX12_WRITE_REG_INSTR;     		// Instruction Reg Write Data
    sendBuffer[5] = address;    					// Register Address
    sendBuffer[6] = (uint8_t) (value & 0xFF);   	// Low register value
    sendBuffer[7] = (uint8_t) (value >> 8);     	// High register value
    sendBuffer[8] = ~((uint8_t)(servoId + 5 + AX12_WRITE_DATA_INSTR + address + sendBuffer[6] + sendBuffer[7]));        // Check Sum
    BOARD_USARTSendBuff.Out(sendBuffer, 9);
	//BOARD_USARTSendBuff(sendBuffer, 9);
	// check collision
	// wait response
	// check checksum
	// check recvId
	// assign value for status
    return AX12_OK;
}


uint8_t  	AX_12::SendWriteRegMultiBytes(uint8_t servoId, uint8_t address, uint8_t value[], uint8_t length)
{
    sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;                		// ID
    sendBuffer[3] = 3 + length;                		// Length
    sendBuffer[4] = AX12_WRITE_REG_INSTR;     		// Instruction Reg Write Data
    sendBuffer[5] = address;    					// Register Address	
	int i = 0;
	int sum = (uint8_t)(servoId + 3 + length + AX12_WRITE_DATA_INSTR + address);
	while(value[i] != NULL){
		sendBuffer[i+6] = (uint8_t) (value[i] & 0xFF);		
		sum = (uint8_t)(sum + sendBuffer[i+6]);		// Check Sum
		i++;
	}
	sendBuffer[i+6] = ~((uint8_t) sum);
    BOARD_USARTSendBuff.Out(sendBuffer, i+7);	 
	// check collision
	// wait response
	// check checksum
	// check recvId
	// assign value for status */
    return AX12_OK;	
}

/**
  * @brief Send ACTION instruction to a AX-12 servo and wait its response
  * @param servoId ID of the servo receiving the instruction
  * @retval AX12_COLLISION Collision is detected while sendding the instruction
  * @retval AX12_NO_RESPONSE Doesn't receive response from the servo receiving the instruction
  * @retval AX12_CHECKSUM_ERROR Receive a packet with checksum error
  * @retval AX12_WRONG_ID Receive a packet with the ID differ with the received servo
  * @retval AX12_OK Send the instruction and receive response successful
  */
uint8_t		AX_12::SendAction(uint8_t servoId){
    sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;      	// ID
    sendBuffer[3] = 2;            	// Length
    sendBuffer[4] = 0x05;         	// Instruction Action
    sendBuffer[5] = ~((uint8_t)(servoId + 2 + 5));            // Check Sum
    BOARD_USARTSendBuff.Out(sendBuffer, 6);
    return AX12_OK;
}

/**
  * @brief Send WRITE DATA instruction to a AX-12 servo and wait its response
  * @param servoId ID of the servo receiving the instruction
  * @param status Status information extracted from the response packet 
  * @retval AX12_COLLISION Collision is detected while sendding the instruction
  * @retval AX12_NO_RESPONSE Doesn't receive response from the servo receiving the instruction
  * @retval AX12_CHECKSUM_ERROR Receive a packet with checksum error
  * @retval AX12_WRONG_ID Receive a packet with the ID differ with the received servo
  * @retval AX12_OK Send the instruction and receive response successful
  */
uint8_t     AX_12::SendReset(uint8_t servoId){
    uint8_t sendBuffer[6];
    sendBuffer[0] = 0x0ff;
    sendBuffer[1] = 0x0ff;
    sendBuffer[2] = servoId;         // ID
    sendBuffer[3] = 2;               // Length
    sendBuffer[4] = 0x06;            // Instruction Reset
    sendBuffer[5] = ~((uint8_t)(servoId + 2 + 6));            // Check Sum
    BOARD_USARTSendBuff.Out(sendBuffer, 6);
    return AX12_OK;
}

uint8_t     AX_12::SendSyncWrite(uint8_t value[], uint8_t length)
{
	return AX12_OK;
}

void	AX_12::Write(uint8_t servoId, uint8_t address, uint8_t value)
{
	AX12.SendWriteDataOneByte(servoId, address, value);
	return ;
} 

void	AX_12::Write_16bits(uint8_t servoId, uint8_t address, uint16_t value)
{
	AX12.SendWriteDataTwoBytes(servoId, address, value);
	return;
}

uint8_t     AX_12::SendSyncWriteOneByte(uint16_t value[], uint8_t address, uint8_t numServo)
{
	uint8_t b, N = 0;
	sendBuffer[0] = 0x0ff;
	sendBuffer[1] = 0x0ff;
	sendBuffer[2] = 0xfe;									// ID broacast
	sendBuffer[3] = 3 * numServo + 4;						// length of data
	sendBuffer[4] = AX12_SYNC_WRITE_INSTR;					// Instruction Sync
	sendBuffer[5] = address;
	sendBuffer[6] = 0x02;
	for (int i = 0; i < numServo * 2; i += 2) {
		sendBuffer[7 + N] = value[i];
		sendBuffer[8 + N] = (uint8_t) (value[i + 1] & 0xFF);
		sendBuffer[9 + N] = (uint8_t) (value[i + 1] >> 8) ;
		b = b + sendBuffer[7 + N] + sendBuffer[8 + N] + sendBuffer[9 + N];
		N += 3;
	}
	sendBuffer[7 + numServo * 3] = ~((uint8_t) (0xfe + sendBuffer[3] + AX12_SYNC_WRITE_INSTR + address + 0x02 + b));        // Check Sum
	BOARD_USARTSendBuff.Out(sendBuffer, 8 + numServo * 3);
	return AX12_OK;
}

uint8_t     AX_12::SendSyncWriteTwoByte(uint16_t value[], uint8_t address, uint8_t numServo)
{
	uint8_t b, N = 0;
	sendBuffer[0] = 0x0ff;
	sendBuffer[1] = 0x0ff;
	sendBuffer[2] = 0xfe;								// ID broacast
	sendBuffer[3] = 5 * numServo + 4;					// length of data
	sendBuffer[4] = AX12_SYNC_WRITE_INSTR;				// Instruction Sync
	sendBuffer[5] = address;
	sendBuffer[6] = 0x04;
	for (int i = 0; i < numServo * 3; i += 3) {
		sendBuffer[7 + N] = value[i];
		sendBuffer[8 + N] = (uint8_t) (value[i + 1] & 0xFF);
		sendBuffer[9 + N] = (uint8_t) (value[i + 1] >> 8);
		sendBuffer[10 + N] = (uint8_t) (value[i + 2] & 0xFF);
		sendBuffer[11 + N] = (uint8_t) (value[i + 2] >> 8);
		b = b + sendBuffer[7 + N] + sendBuffer[8 + N] + sendBuffer[9 + N] + sendBuffer[10 + N] + sendBuffer[11 + N];
		N += 5;
	}
	sendBuffer[5 * numServo + 7] = ~((uint8_t) (0xfe + sendBuffer[3] + AX12_SYNC_WRITE_INSTR + address + 0x04 + b));        // Check Sum
	BOARD_USARTSendBuff.Out(sendBuffer, 5 * numServo + 8);
	return AX12_OK;
}

//=========================================================//
//====================Support Functions====================//
//=========================================================//

void	AX_12::CheckCollision(uint8_t* status)
{
	while(*status == AX12_COLLISION)
		AX12.WaitResponse(status);
	*status = AX12_COLLISION;
	return;	  
}


void	AX_12::WaitResponse(uint8_t *status)
{
	while(*status == AX12_COLLISION){}
	return;	  
}


/**
  * @brief Send WRITE DATA instruction to a AX-12 servo and wait its response
  * @param servoId ID of the servo receiving the instruction
  * @param status Status information extracted from the response packet 
  * Bit 0 : Set to 1 if the voltage is out of the operating voltage range as defined in the control table
  * Bit 1 : Set as 1 if the Goal Position is set outside of the range between CW Angle Limit and CCW Angle Limit.
  * Bit 2 : Set to 1 if the internal temperature of the Dynamixel unit is above the operating temperature range as defined in the control table.
  * Bit 3 : Set to 1 if the instruction sent is out of the defined range.
  * Bit 4 : Set to 1 if the checksum of the instruction packet is incorrect.
  * Bit 5 : Set to 1 if the specified maximum torque can't control the applied load.
  * Bit 6 : Set to 1 if an undefined instruction is sent or an action instruction is sent without a Reg_Write instruction.
  * Bit 7 : 0
  */
void	AX_12::CheckSum(uint8_t servoId, uint8_t length, uint8_t instruction, uint8_t parameter[], uint8_t sum, uint8_t *status)
{
	if(recvBuffer[0] != 0xFF || recvBuffer[1] != 0xFF || recvBuffer[2] != servoId || recvBuffer[3] != length) {
		AX12.SetLed(servoId, 1); 
		AX12.SetLed(servoId, 0);
		AX12.SetLed(servoId, 1);
		AX12.SetLed(servoId, 0);
	}
	if(recvBuffer[4] != 0) {
		if((recvBuffer[4] & 0x01) != 0)  {
			// turn off servo
		}
		else if((recvBuffer[4] & 0x02) != 0){
			// turn off servo
		}
		else if((recvBuffer[4] & 0x04) != 0) {}
		else if((recvBuffer[4] & 0x08) != 0){}
		else if((recvBuffer[4] & 0x10) != 0){}
		else if((recvBuffer[4] & 0x20) != 0){}
		else if((recvBuffer[4] & 0x40) != 0){}
		else {}
	}

	int i = 0;
	while(recvBuffer[i] != NULL){
		i++;
	}
	if(recvBuffer[i] != sum){
		// checksum status packet error
	}
	return; 
}

void	AX_12::CheckRecvId(uint8_t servoId, uint8_t *status)
{
	if((*status == AX12_NO_RESPONSE)||(*status == AX12_CHECKSUM_ERROR)) {
		AX12.SetLed(servoId, 0x10);
		return;
	}
	if(servoId != recvBuffer[2]) {
		*status = AX12_WRONG_ID;
		AX12.SetLed(servoId, 0x10);
	}
	return;
}

void	AX_12::AssignValueStatus(uint8_t *status)
{
	if((*status == AX12_NO_RESPONSE)||(*status == AX12_WRONG_ID) || (*status == AX12_CHECKSUM_ERROR)){
		return;
	}
	else
		*status = AX12_OK;	 
	return;	
}
